#include "cmsis_os.h"
#include "stdio.h"
#include "string.h"

#include "usb_comm_app.h"
#include "usbd_cdc_if.h"

#include "leg.h"

extern osMutexId usb_buf_mutexHandle;
extern osSemaphoreId usb_pack_SemHandle;

uint8_t usb_rx_buf[1024];

Tar_point_t tar_point[4];

Leg_index_e get_legIndex_from_str(uint8_t* buf)
{
	Leg_index_e leg_index = 0;
	if(strcmp(buf, "lf") == 0)
	{
		leg_index = LEG_INDEX_LF;
	} else if(strcmp(buf, "rf") == 0)
	{
		leg_index = LEG_INDEX_RF;
	} else if(strcmp(buf, "lb") == 0)
	{
		leg_index = LEG_INDEX_LB;
	} else if(strcmp(buf, "rb") == 0)
	{
		leg_index = LEG_INDEX_RB;
	}
	return leg_index;
}

void update_tar_pos(uint8_t* buf)
{
	
	// usbprint("received %s \r\n", buf);
	buf += 1;
	buf[2] = 0;

	float x, y;
	sscanf(buf+3, "%f,%f", &x, &y);
	Leg_index_e leg_index = get_legIndex_from_str(buf);
	usbprint("%d: %f, %f\r\n", leg_index, x, y);
	
	tar_point[leg_index].x = x*100;
	tar_point[leg_index].y = y*100;
}

void Usb_RxCplt_CallBack(uint8_t* buf, uint32_t len)
{
	osStatus res = osMutexWait(usb_buf_mutexHandle, 0xff);
	if (res == osOK)
	{
		memcpy(usb_rx_buf, buf, len);
		// usb_rx_buf[len] = 0;
		// update_tar_pos(usb_rx_buf);
		osMutexRelease(usb_buf_mutexHandle);
		osSemaphoreRelease(usb_pack_SemHandle);
	}
}

void get_tar_pos(Leg_index_e leg_index, Tar_point_t* ptar_point)
{
	ptar_point->x = tar_point[leg_index].x;
	ptar_point->y = tar_point[leg_index].y;
}

#include "usr_conf.h"

void Comm_USB(void const * argument)
{
	/* Used in osDelayUntil */
	uint32_t PreviousWakeTime;
	for (char i = 0; i < 4; i++)
	{
		tar_point[i].x = 3;
		tar_point[i].y = LEG_ZEROY;
	}
	
	for(;;)
	{
//			usbprint("1234\n");

		/* save wakeup time */
		// 信号量给初值！！！！！！！！！！！！！！！！！！！！！
		PreviousWakeTime = osKernelSysTick();
		osSemaphoreWait(usb_pack_SemHandle, 0xffffffff);
		osMutexWait(usb_buf_mutexHandle, 0xffffffff);
		update_tar_pos(usb_rx_buf);
		osMutexRelease(usb_buf_mutexHandle);
		osDelayUntil(&PreviousWakeTime, 2);// run every 2ms
	}
}
